{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Tutorial: CommonRoad-io\n",
    "## Reading, Modifying, and Writing Common Scenarios\n",
    "\n",
    "This tutorial shows how CommonRoad XML-files can be read, modified, visualized, and stored. To start with, a CommonRoad XML-file consists of a **Scenario** and a **PlanningProblemSet**:\n",
    "* A **Scenario** represents the environment including a **LaneletNetwork** and a set of **DynamicObstacle** and **StaticObstacle**.\n",
    "    * A **LaneletNetwork** is built from lane segments (**Lanelet**), that can be connected arbitrarily.\n",
    "* A **PlanningProblemSet** contains one **PlanningProblem** for every ego vehicle in the **Scenario**, consisting of an **initial position** and a **GoalRegion** that has to be reached."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0. Preparation\n",
    "* Before you proceed any further, make sure you have skimmed through [CommonRoad API](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html#modules) to gain an overall view of the funtionalities provided by CommonRoad modules. You may need to refer to it for implementation details throughout this tutorial.\n",
    "\n",
    "* Additional documentations on **CommonRoad XML format, Cost Functions, Vehicle Models, etc.** can be found at [CommonRoad](https://commonroad.in.tum.de/) under section **Documentation**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Read XML file\n",
    "\n",
    "As documented in [CommonRoadFileReader](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_reader), the **CommonRoadFileReader** reads in a CommonRoad XML file, and its **open()** method returns a **Scenario** and a **PlanningProblemSet** object:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython import display\n",
    "\n",
    "# import functions to read xml file and visualize commonroad objects\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad.visualization.draw_dispatch_cr import draw_object\n",
    "\n",
    "# generate path of the file to be opened\n",
    "file_path = \"../../scenarios/tutorial/ZAM_Tutorial-1_1_T-1.xml\"\n",
    "\n",
    "# read in the scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# plot the scenario for 40 time step, here each time step corresponds to 0.1 second\n",
    "for i in range(0, 40):\n",
    "    # uncomment to clear previous graph\n",
    "    # display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(20, 10))\n",
    "    # plot the scenario at different time step\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    # plot the planning problem set\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Modify XML file\n",
    "\n",
    "It is possible to modify existing CommonRoad scenarios to costumize them to one's need. First, we would like to add a static obstacle to the scenario with the following specification:\n",
    "\n",
    "    - obstacle type: parked vehicle\n",
    "    - obstacle shape: rectangle, with a width of 2.0m and a length of 4.5m\n",
    "    - initial state:\n",
    "        - position: (30, 3.5) m\n",
    "        - orientation: 0.02 rad\n",
    "* obstacle id: since every object in the scenario must have a unique ID, we can use the member function **generate_object_id** of **Scenario** class to generate a unique ID for the object.\n",
    "\n",
    "As documented in [StaticObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.StaticObstacle), we need to provide `obstacle_id, obstacle_type, obstacle_shape, initial_state` to construct a static obstacle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "is_executing": false
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# import necesary classes from different modules\n",
    "from commonroad.geometry.shape import Rectangle\n",
    "from commonroad.scenario.obstacle import StaticObstacle, ObstacleType\n",
    "from commonroad.scenario.trajectory import State\n",
    "\n",
    "# read in the scenario and planning problem set\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# generate the static obstacle according to the specification, refer to API for details of input parameters\n",
    "static_obstacle_id = scenario.generate_object_id()\n",
    "static_obstacle_type = ObstacleType.PARKED_VEHICLE\n",
    "static_obstacle_shape = Rectangle(width = 2.0, length = 4.5)\n",
    "static_obstacle_initial_state = State(position = np.array([30.0, 3.5]), orientation = 0.02, time_step = 0)\n",
    "\n",
    "# feed in the required components to construct a static obstacle\n",
    "static_obstacle = StaticObstacle(static_obstacle_id, static_obstacle_type, static_obstacle_shape, static_obstacle_initial_state)\n",
    "\n",
    "# add the static obstacle to the scenario\n",
    "scenario.add_objects(static_obstacle)\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    # uncomment to clear previous graph\n",
    "    # display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As can be seen, we have added a new static obstacle to the scenario. We can further add a dynamic obstacle with the following specifications:\n",
    "\n",
    "    - obstacle type: car\n",
    "    - obstacle shape: rectangle with a width of 1.8m and a length of 4.3m\n",
    "    - initial state:\n",
    "        - position: (50, 0.0) m\n",
    "        - orientation: 0.00 rad\n",
    "        - velocity: 22 m/s along x axis\n",
    "    - we assume that the dynamic obstacle drives with constant velocity.\n",
    "\n",
    "As documented in [DynamicObstacle](https://commonroad.in.tum.de/static/docs/commonroad-io/api/scenario.html#commonroad.scenario.obstacle.DynamicObstacle), we need to pass in a **Prediction** object which in this case is a **TrajectoryPrediction** object. Its generation goes as follows:\n",
    "    1. compute all subsequent states for the dynamic obstacle\n",
    "    2. create a Trajectory from these states\n",
    "    3. create a TrajectoryPrediction from this trajectory and obstacle shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import necesary classes from different modules\n",
    "from commonroad.scenario.obstacle import DynamicObstacle\n",
    "from commonroad.scenario.trajectory import Trajectory\n",
    "from commonroad.prediction.prediction import TrajectoryPrediction\n",
    "\n",
    "dynamic_obstacle_id = scenario.generate_object_id()\n",
    "dynamic_obstacle_type = ObstacleType.CAR\n",
    "# initial state has a time step of 0\n",
    "dynamic_obstacle_initial_state = State(position = np.array([50.0, 0.0]),\n",
    "                                       velocity = 22,\n",
    "                                       orientation = 0.02,\n",
    "                                       time_step = 0)\n",
    "\n",
    "# generate the states for the obstacle for time steps 1 to 40 by assuming constant velocity\n",
    "state_list = []\n",
    "for i in range(1, 40):\n",
    "    # compute new position\n",
    "    new_position = np.array([dynamic_obstacle_initial_state.position[0] + scenario.dt * i * 22, 0])\n",
    "    # create new state\n",
    "    new_state = State(position = new_position, velocity = 22,orientation = 0.02, time_step = i)\n",
    "    # add new state to state_list\n",
    "    state_list.append(new_state)\n",
    "\n",
    "# create the trajectory of the obstacle, starting at time step 1\n",
    "dynamic_obstacle_trajectory = Trajectory(1, state_list)\n",
    "\n",
    "# create the prediction using the trajectory and the shape of the obstacle\n",
    "dynamic_obstacle_shape = Rectangle(width = 1.8, length = 4.3)\n",
    "dynamic_obstacle_prediction = TrajectoryPrediction(dynamic_obstacle_trajectory, dynamic_obstacle_shape)\n",
    "\n",
    "# generate the dynamic obstacle according to the specification\n",
    "dynamic_obstacle_id = scenario.generate_object_id()\n",
    "dynamic_obstacle_type = ObstacleType.CAR\n",
    "dynamic_obstacle = DynamicObstacle(dynamic_obstacle_id, \n",
    "                                   dynamic_obstacle_type, \n",
    "                                   dynamic_obstacle_shape, \n",
    "                                   dynamic_obstacle_initial_state, \n",
    "                                   dynamic_obstacle_prediction)\n",
    "\n",
    "# add dynamic obstacle to the scenario\n",
    "scenario.add_objects(dynamic_obstacle)\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    # uncomment to clear previous graph\n",
    "    # display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Write XML file\n",
    "After we have modified the scenario, we would like to save the changes and write the **Scenario** and **PlanningProblemSet** to a CommonRoad XML file. [CommonRoadFileWriter](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.file_writer) helps us with this purpse.\n",
    "Note that we did not modify the **PlanningProblemSet** in this tutorial."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import necesary classes from different modules\n",
    "from commonroad.common.file_writer import CommonRoadFileWriter\n",
    "from commonroad.common.file_writer import OverwriteExistingFile\n",
    "\n",
    "author = 'Max Mustermann'\n",
    "affiliation = 'Technical University of Munich, Germany'\n",
    "source = ''\n",
    "tags = 'highway multi_lane no_oncoming_traffic parallel_lanes comfort'\n",
    "\n",
    "# write new scenario\n",
    "fw = CommonRoadFileWriter(scenario, planning_problem_set, author, affiliation, source, tags)\n",
    "\n",
    "filename = \"../../solutions/ZAM_Tutorial-1_2_T-1.xml\"\n",
    "fw.write_to_file(filename, OverwriteExistingFile.ALWAYS)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can open our stored file again to check if everything is correct:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "file_path = \"../../solutions/ZAM_Tutorial-1_2_T-1.xml\"\n",
    "\n",
    "scenario, planning_problem_set = CommonRoadFileReader(file_path).open()\n",
    "\n",
    "# plot the scenario for each time step\n",
    "for i in range(0, 40):\n",
    "    # uncomment to clear previous graph\n",
    "    # display.clear_output(wait=True)\n",
    "    plt.figure(figsize=(25, 10))\n",
    "    draw_object(scenario, draw_params={'time_begin': i})\n",
    "    draw_object(planning_problem_set)\n",
    "    plt.gca().set_aspect('equal')\n",
    "    plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "metadata": {
     "collapsed": false
    },
    "source": []
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}